#include <modules/MonitorModule/SlipMonitor.h>
#include <modules/CtrlModule/PassivePlanner/SlipReactionPlanner.h>
#include "lib/Time.h"
#include "modules/CtrlModule/MarkPosePubModel.h"

namespace behavior_controller
{
SlipMonitor slip_monitor;

void SlipMonitor::run()
{
    if (slip_reaction_planner.finished) //执行响应后不会再进入，直至finished为true
    {
        double time = 2;
        double slip_distance_square = 0.0004; // 里程计位置变化和机器人定位位置变化距离之差的平方， 0.02m

        if (last_time < 1e-4)
        {
            last_time = Time::Now();
            last_odom_pose = PointF(DataSet::odom_data.position_x(), DataSet::odom_data.position_y());
            last_robot_pose = PointF(DataSet::robot_pose.x(), DataSet::robot_pose.y());
            //last_odom_raw_angle= DataSet::odom_data.yaw();
            //last_robot_raw_angle= DataSet::robot_pose.angle();
        }

        if (Time::Now() - last_time >= time)
        {
            PointF odom_pose = PointF(DataSet::odom_data.position_x(), DataSet::odom_data.position_y());
            PointF robot_pose_ = PointF(DataSet::robot_pose.x(), DataSet::robot_pose.y());

            if ((odom_pose.DistanceSquare(last_odom_pose) - robot_pose_.DistanceSquare(last_robot_pose))
                >= slip_distance_square)
            {
                DataSet::ctrl_state = SLIP_PLAN;
                slip_reaction_planner.finished = false;
                PointF robot_pose_tmp(DataSet::robot_pose.x(), DataSet::robot_pose.y());
                std::vector<PointF> pose_array;
                pose_array.emplace_back(robot_pose_tmp);
                mark_pose_pub_mod.MarkPosePub(pose_array);
                std::cout << "slip  bad !" << std::endl;
            }
            last_time = 0;
        }
    }
}
}